#include "radar.hpp"

#include "can_configurator.h"
#include "can_transreceiver.h"
#include "chuansu/chuansu_can_adapter.h"
#include "manku/manku_can_adapter.h"
#include "muduo/net/EventLoop.h"
#include "muduo/net/EventLoopThread.h"
#include "nezha/nezha_can_adapter.h"
#include "zhongtong/zhongtong_can_adapter.h"
#include "ziyan/ziyan_can_adapter.h"
using namespace sensor::radar;

static muduo::net::EventLoopThread RadarTransreceiverThread;
static muduo::net::EventLoopThread RadarAdapterThread;

RadarComponent::RadarComponent(const rclcpp::NodeOptions& options)
    : rclcpp::Node("radar", options)
{
    auto logger = rclcpp::get_logger("sensor::radar");
    RCLCPP_INFO(logger, "sensor::radar start!");

    setupConfigures();
    setupTansreceivers();
    setupAdapters();

    for (auto& dev : can_devs_) {
        dev.second->start();
    }

    RCLCPP_INFO(logger, "sensor::radar init done!");
}

void RadarComponent::setupConfigures()
{
    CanConfigurator cfg0("can0");
    cfg0.setup();
    CanConfigurator cfg1("can1");
    cfg1.setup();
}

void RadarComponent::setupTansreceivers()
{
    auto loop = RadarTransreceiverThread.startLoop();
    can_devs_ = {
        { "can0", std::make_shared<sensor::radar::CanTransreceiver>("can0", loop) },
        { "can1", std::make_shared<sensor::radar::CanTransreceiver>("can1", loop) },
    };
}

void RadarComponent::setupAdapters()
{
    auto loop1 = RadarAdapterThread.startLoop();
    adapters_["zhongtong"] = std::make_shared<ZTCanAdapter>(can_devs_);
    adapters_["manku"] = std::make_shared<MankuCanAdapter>(can_devs_);
    // rclcpp::spin(adapters_["Nezha"]);
    loop1->runInLoop([&] { rclcpp::spin(adapters_["zhongtong"]);
        rclcpp::spin(adapters_["manku"]); });
}

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(sensor::radar::RadarComponent)
